#include <stddef.h>
#include <pmmintrin.h>
#include<stdio.h>
#include "solver.h"

#define IX(i,j) ((j)+(n+2)*(i))
#define SWAP(x0,x) {float * tmp=x0;x0=x;x=tmp;}

typedef enum { NEITHER = 0, HORIZONTAL = 1, VERTICAL = 2 } boundary;

static void add_source(unsigned int n, float * x, const float * s, float dt)
{
    int size = (n + 2) * (n + 2);
    __m128 dtPaked;
    dtPaked =  _mm_load1_ps(&dt);
    for (unsigned int i = 0; i <= size-3; i+=4)
    {
        __m128 v,res;
        v   = _mm_load_ps(&s[i]);
        res = _mm_load_ps(&x[i]);
        v   = _mm_mul_ps(v,dtPaked);
        res = _mm_add_ps(res,v);
              _mm_store_ps(&x[i],res);
    }
}

static void set_bnd(unsigned int n, boundary b, float * x)
{
    for (unsigned int i = 1; i <= n; i++) {
        x[IX(0, i)]     = b == 1 ? -x[IX(1, i)] : x[IX(1, i)];
        x[IX(n + 1, i)] = b == 1 ? -x[IX(n, i)] : x[IX(n, i)];
        x[IX(i, 0)]     = b == 2 ? -x[IX(i, 1)] : x[IX(i, 1)];
        x[IX(i, n + 1)] = b == 2 ? -x[IX(i, n)] : x[IX(i, n)];
    }
    x[IX(0, 0)]         = 0.5f * (x[IX(1, 0)]     + x[IX(0, 1)]);
    x[IX(0, n + 1)]     = 0.5f * (x[IX(1, n + 1)] + x[IX(0, n)]);
    x[IX(n + 1, 0)]     = 0.5f * (x[IX(n, 0)]     + x[IX(n + 1, 1)]);
    x[IX(n + 1, n + 1)] = 0.5f * (x[IX(n, n + 1)] + x[IX(n + 1, n)]);
}

static void lin_solve(unsigned int n, boundary b, float * x, const float * x0, float a, float c)
{
    __m128 xUpper;
    __m128 xLower;
    __m128 xPrev; 
    __m128 xPost; 
    __m128 xCenter;
    __m128 x0Prev;
    __m128 xDiffused;
    __m128 xTempCenter;
    __m128 cteA; 
    __m128 cteC; 
    
    cteA = _mm_load1_ps(&a); //cargo la cte a
    cteC = _mm_load1_ps(&c); //cargo la cte c    
    //hacer la cruz, por eso son 5
    for (unsigned int k = 0; k < 20; k++) 
    {
        for (unsigned int i = 1; i <= n; i++) 
        {
            xUpper    = _mm_load_ps(&x[IX(i-1, 0)]);
            xCenter    = _mm_load_ps(&x[IX(i, 0)]);
            xPost     = _mm_load_ps(&x[IX(i, 4)]);
            xLower    = _mm_load_ps(&x[IX(i+1, 0)]);
            x0Prev    = _mm_load_ps(&x0[IX(i, 0)]);
            
            xDiffused = _mm_add_ps(xUpper,xLower);//sumo los de arriba y los de abajo
            xCenter    = _mm_shuffle_ps(xCenter,xCenter,0x93);//los corro a la derecha para sumar el de la izquierda
            xDiffused = _mm_add_ps(xDiffused,xCenter);//sumo los de la izquierda (borde tambien, no importa, despues hago set_bounds)
            xCenter    = _mm_shuffle_ps(xCenter,xCenter,0x39);//vuelvo a la posicion anterior, hacia la izquierda
            xCenter    = _mm_sub_ss(xCenter,xCenter);//pongo en cero el borde para que no me sume nada al ultimo elemento
            xCenter    = _mm_shuffle_ps(xCenter,xCenter,0x39);//los corro a la izquierda de nuevo
            xDiffused = _mm_add_ps(xDiffused,xCenter);//sumo los de la derecha (al ultimo le sumo cero)
            xDiffused = _mm_shuffle_ps(xDiffused,xDiffused,0x93);//corro a la derecha para dejar el ultimo primero
            xDiffused = _mm_add_ss(xDiffused,xPost);//le sumo el que seria el de la derecha del ultimo
            xDiffused = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//corro a la izquierda para acomodar como estaban originalmente
            xDiffused = _mm_mul_ps(xDiffused,cteA);//multiplico por cte a
            xDiffused = _mm_add_ps(xDiffused,x0Prev); //sumo los valores de x0
            xDiffused = _mm_div_ps(xDiffused,cteC);//multiplico por cte c
            _mm_store_ps(&x[IX(i, 0)],xDiffused);//guardo
            
            for (unsigned int j = 4; j < n-2; j+=4)
            {
                //preparo para el siguiente 
                xPrev      = _mm_load_ps(&x[IX(i, j-4)]);//lo cargo al reves para ya tenerlo listo para la suma
                xTempCenter = _mm_load_ps(&x[IX(i, j)]); 
                xCenter     = _mm_load_ps(&x[IX(i, j)]);
                xPost      = _mm_load_ps(&x[IX(i, j+4)]);
                xUpper     = _mm_load_ps(&x[IX(i-1, j)]);
                xLower     = _mm_load_ps(&x[IX(i+1, j)]);
                x0Prev     = _mm_load_ps(&x0[IX(i, j)]);
                
                //nomenclatura: ...x] [a,b,c,d] [y...
                xCenter     = _mm_sub_ss(xCenter,xCenter); //pongo a cero el primer elemento de xCenter
                xCenter     = _mm_add_ss(xCenter,xPost); //guardo y en xCenter, queda [y,b,c,d]
                xCenter     = _mm_shuffle_ps(xCenter,xCenter,0x39);//los corro a la izquierda, quedan [b,c,d,y]
                xTempCenter = _mm_shuffle_ps(xTempCenter,xTempCenter,0x93);//los corro a la izquierda, quedan [d,a,b,c]
                xTempCenter = _mm_sub_ss(xTempCenter,xTempCenter); //pongo a cero el primer elemento de xTempCenter
                xTempCenter = _mm_add_ss(xTempCenter,_mm_shuffle_ps(xPrev,xPrev,0x1B)); //guardo x en xTempCenter, queda [x,a,b,c]
                xDiffused  = _mm_add_ps(xTempCenter,xCenter); //sumo y me queda [b+x,c+a,d+b,y+c] (sume prev + post)
                xDiffused  = _mm_add_ps(xDiffused,xUpper); //sumo los de arriba
                xDiffused  = _mm_add_ps(xDiffused,xLower);//sumo los de abajo
                xDiffused  = _mm_mul_ps(xDiffused,cteA);//multiplico por cte a
                xDiffused  = _mm_add_ps(xDiffused,x0Prev); //sumo los valores de x0
                xDiffused  = _mm_div_ps(xDiffused,cteC);//multiplico por cte c
                             _mm_store_ps(&x[IX(i, j)],xDiffused);//guardo
            }

            //preparo para el siguiente 
            xPrev      = _mm_load_ps(&x[IX(i, n-6)]);//lo cargo al reves para ya tenerlo listo para la suma
            xTempCenter = _mm_load_ps(&x[IX(i, n-2)]); 
            xCenter     = _mm_load_ps(&x[IX(i, n-2)]);            
            xUpper     = _mm_load_ps(&x[IX(i-1, n-2)]);
            xLower     = _mm_load_ps(&x[IX(i+1, n-2)]);
            x0Prev     = _mm_load_ps(&x0[IX(i, n-2)]);
            
            xDiffused = _mm_add_ps(xUpper,xLower);
            xCenter    = _mm_shuffle_ps(xCenter,xCenter,0x39);//los corro a la izquierda para sumar el de la derecha
            xDiffused = _mm_add_ps(xDiffused,xCenter);//sumo los de la derecha (borde tambien, no importa, despues hago set_bounds)
            xCenter    = _mm_shuffle_ps(xCenter,xCenter,0x4E);//muevo 2 a la derecha
            xCenter    = _mm_sub_ss(xCenter,xCenter);//pongo en cero el borde para que no me sume nada al ultimo elemento
            xDiffused = _mm_add_ps(xDiffused,xCenter);//sumo los de la derecha (al ultimo le sumo cero)
            xDiffused = _mm_add_ss(xDiffused,_mm_shuffle_ps(xPrev,xPrev,0x1B));//le sumo el que seria el de la izquierda del primero
            xDiffused = _mm_mul_ps(xDiffused,cteA);//multiplico por cte a
            xDiffused = _mm_add_ps(xDiffused,x0Prev); //sumo los valores de x0
            xDiffused = _mm_div_ps(xDiffused,cteC);//multiplico por cte c
            _mm_store_ps(&x[IX(i, n-2)],xDiffused);//guardo
        }
        set_bnd(n, b, x);
    }
    
//Experimento con correccion numerica:
/*
    __m128 xUpper;
    __m128 xLower;
    __m128 xPrev; 
    __m128 xPost; 
    __m128 xCenter;
    __m128 x0Prev;
    __m128 xDiffused;
    __m128 xTempCenter;
    __m128 cteA; 
    __m128 cteC; 
    
    cteA = _mm_load1_ps(&a); //cargo la cte a
    cteC = _mm_load1_ps(&c); //cargo la cte c    
    //hacer la cruz, por eso son 5
    for (unsigned int k = 0; k < 20; k++) 
    {
        for (unsigned int i = 1; i <= n; i++) 
        {
            xUpper      = _mm_load_ps(&x[IX(i-1, 0)]);
            xCenter     = _mm_load_ps(&x[IX(i, 0)]);
            xTempCenter = _mm_load_ps(&x[IX(i, 0)]); 
            xPost       = _mm_load_ps(&x[IX(i, 4)]);
            xLower      = _mm_load_ps(&x[IX(i+1, 0)]);
            x0Prev      = _mm_load_ps(&x0[IX(i, 0)]);
            
            xDiffused  = _mm_add_ps(xUpper,xLower);//sumo los de arriba y los de abajo
            xCenter    = _mm_sub_ss(xCenter,xCenter);//pongo en cero el borde para cargar el y
            xCenter    = _mm_add_ss(xCenter,xPost);//le sumo el que seria el de la derecha del ultimo
            xCenter    = _mm_shuffle_ps(xCenter,xCenter,0x39);//los corro a la izquierda para sumar el de la derecha
            xDiffused  = _mm_add_ps(xDiffused,xCenter);//sumo los de la derecha

            xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el primer elemento no borde al principio (hacia la izquierda)
            x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
            xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del primero (el borde)
            xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
            xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
            xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c

            xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el segundo elemento no borde al principio (hacia la izquierda)
            x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
            xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del primero (el borde)
            xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
            xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
            xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c

            xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el tercer elemento no borde al principio (hacia la izquierda)
            x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
            xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del primero (el borde)
            xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
            xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
            xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
            
            xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//Acomodo original
            //x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
                         _mm_store_ps(&x[IX(i, 0)],xDiffused);//guardo
            
            for (unsigned int j = 4; j < n-2; j+=4)
            {
                //preparo para el siguiente 
                xPrev       = _mm_load_ps(&x[IX(i, j-4)]);//lo cargo al reves para ya tenerlo listo para la suma
                xTempCenter = _mm_load_ps(&x[IX(i, j)]); 
                xCenter     = _mm_load_ps(&x[IX(i, j)]);
                xPost       = _mm_load_ps(&x[IX(i, j+4)]);
                xUpper      = _mm_load_ps(&x[IX(i-1, j)]);
                xLower      = _mm_load_ps(&x[IX(i+1, j)]);
                x0Prev      = _mm_load_ps(&x0[IX(i, j)]);
                
                //nomenclatura: ...x] [a,b,c,d] [y...
                xDiffused   = _mm_add_ps(xUpper,xLower);//sumo los de arriba y los de abajo
                xCenter     = _mm_sub_ss(xCenter,xCenter);//pongo en cero el borde para cargar el y
                xCenter     = _mm_add_ss(xCenter,xPost);//le sumo el que seria el de la derecha del ultimo
                xCenter     = _mm_shuffle_ps(xCenter,xCenter,0x39);//los corro a la izquierda para sumar el de la derecha
                xDiffused   = _mm_add_ps(xDiffused,xCenter);//sumo los de la derecha
                xTempCenter = _mm_add_ss(xTempCenter,_mm_shuffle_ps(xPrev,xPrev,0x1B)); //guardo x en xTempCenter, queda [x,a,b,c]

                xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del primero
                xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
                xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
                xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
                xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el segundo elemento al principio (hacia la izquierda)
                x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
    
                xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del segundo elemento
                xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
                xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
                xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
                xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el tercer elemento al principio (hacia la izquierda)
                x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
    
                xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del tercero (el borde)
                xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
                xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
                xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
                xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el cuarto elemento al principio (hacia la izquierda)
                x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia

                xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del cuarto (el borde)
                xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
                xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
                xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
                xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//Acomodo original
                //x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
                             _mm_store_ps(&x[IX(i, j)],xDiffused);//guardo
            }

            //preparo para el siguiente 
            xPrev       = _mm_load_ps(&x[IX(i, n-6)]);//lo cargo al reves para ya tenerlo listo para la suma
            xTempCenter = _mm_load_ps(&x[IX(i, n-2)]); 
            xCenter     = _mm_load_ps(&x[IX(i, n-2)]);            
            xUpper      = _mm_load_ps(&x[IX(i-1, n-2)]);
            xLower      = _mm_load_ps(&x[IX(i+1, n-2)]);
            x0Prev      = _mm_load_ps(&x0[IX(i, n-2)]);
            
            xDiffused   = _mm_add_ps(xUpper,xLower);//sumo los de arriba y los de abajo
            xCenter     = _mm_shuffle_ps(xCenter,xCenter,0x39);//los corro a la izquierda para sumar el de la derecha
            xDiffused   = _mm_add_ps(xDiffused,xCenter);//sumo los de la derecha
            xTempCenter = _mm_add_ss(xTempCenter,_mm_shuffle_ps(xPrev,xPrev,0x1B)); //guardo x en xTempCenter, queda [x,a,b,c]

            xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del primero
            xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
            xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
            xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
            xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el segundo elemento al principio (hacia la izquierda)
            x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia

            xDiffused   = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del segundo
            xDiffused   = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
            xDiffused   = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
            xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
            xDiffused   = _mm_shuffle_ps(xDiffused,xDiffused,0x39);//pongo el tercer elemento al principio (hacia la izquierda)
            x0Prev      = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia

            xDiffused  = _mm_add_ss(xDiffused,xTempCenter);//le sumo el que seria el de la izquierda del tercero
            xDiffused  = _mm_mul_ss(xDiffused,cteA);//multiplico por cte a
            xDiffused  = _mm_add_ss(xDiffused,x0Prev); //sumo los valores de x0
            xTempCenter = xDiffused  = _mm_div_ss(xDiffused,cteC);//multiplico por cte c
            xDiffused  = _mm_shuffle_ps(xDiffused,xDiffused,0x4E);//Acomodo original
            //x0Prev     = _mm_shuffle_ps(x0Prev,x0Prev,0x39);//hago la correspondencia
                         _mm_store_ps(&x[IX(i, n-2)],xDiffused);//guardo
        }
        set_bnd(n, b, x);
    }
*/
}

static void diffuse(unsigned int n, boundary b, float * x, const float * x0, float diff, float dt)
{
    float a = dt * diff * n * n;
    lin_solve(n, b, x, x0, a, 1 + 4 * a);
}

static void advect(unsigned int n, boundary b, float * d, const float * d0, const float * u, const float * v, float dt)
{
    int i0[4], i1[4], j0[4], j1[4];
    //float x[4], y[4], s0[4], t0[4], s1[4], t1[4];
    
    __m128 dt0Packed; 
    __m128 minPacked; 
    __m128 maxPacked;
    __m128 iPacked; 
    __m128 jPacked; 

    __m128 xPacked; 
    __m128 yPacked; 
    __m128 s1Packed; 
    __m128 s0Packed; 
    __m128 t1Packed; 
    __m128 t0Packed;
    __m128 dAdvected;
    __m128 dAux0Advected;
    __m128 dAux1Advected;
    __m128 aux0Gather;
    __m128 aux1Gather;
    
    __m128i i0Packed;
    __m128i j0Packed;
    __m128i i1Packed;
    __m128i j1Packed;
   
    dt0Packed = _mm_set1_ps(dt * n);
    minPacked = _mm_set1_ps(0.5f);
    maxPacked = _mm_set1_ps(n + 0.5f);


    for (unsigned int i = 1; i <= n; i++) 
    {
        iPacked = _mm_set1_ps((float) i);

        for (unsigned int j = 0; j <= n-2; j+=4) 
        {
            jPacked  = _mm_set_ps((float) (j+3),(float) (j+2),(float) (j+1),(float) j);
            
            xPacked  = _mm_load_ps(&u[IX(i, j)]);                      //x = i - dt0 * u[IX(i, j)];
            xPacked  = _mm_mul_ps(xPacked,dt0Packed);                  //
            xPacked  = _mm_sub_ps(iPacked,xPacked);                    // 
            xPacked  = _mm_max_ps(xPacked,minPacked);                  //if (x < 0.5f) { x = 0.5f;} 
            xPacked  = _mm_min_ps(xPacked,maxPacked);                  //else if (x > n + 0.5f) { x = n + 0.5f;} 
            i0Packed = _mm_cvttps_epi32(xPacked);                       //i0 = (int) x;
            i1Packed = _mm_add_epi32(_mm_set1_epi32(1),i0Packed);      //i1 = i0 + 1; 
            s1Packed = _mm_sub_ps(xPacked,_mm_cvtepi32_ps(i0Packed));  //s1 = x - i0; 
            s0Packed = _mm_sub_ps(_mm_set1_ps(1.0f),s1Packed);         //s0 = 1 - s1;

                       //_mm_store_ps(&x[0],xPacked);
                       _mm_store_si128((__m128i *)&i0[0],i0Packed);
                       _mm_store_si128((__m128i *)&i1[0],i1Packed);
                       //_mm_store_ps(&s0[0],s0Packed);
                       //_mm_store_ps(&s1[0],s1Packed);


            yPacked  = _mm_load_ps(&v[IX(i, j)]);
            yPacked  = _mm_mul_ps(yPacked,dt0Packed);
            yPacked  = _mm_sub_ps(jPacked,yPacked);
            yPacked  = _mm_max_ps(yPacked,minPacked);
            yPacked  = _mm_min_ps(yPacked,maxPacked);
            j0Packed = _mm_cvttps_epi32(yPacked);
            j1Packed = _mm_add_epi32(_mm_set1_epi32(1),j0Packed);
            t1Packed = _mm_sub_ps(yPacked,_mm_cvtepi32_ps(j0Packed));
            t0Packed = _mm_sub_ps(_mm_set1_ps(1.0f),t1Packed);
            
                       //_mm_store_ps(&y[0],yPacked);
                       _mm_store_si128((__m128i *)&j0[0],j0Packed);
                       _mm_store_si128((__m128i *)&j1[0],j1Packed);
                       //_mm_store_ps(&t0[0],t0Packed);
                       //_mm_store_ps(&t1[0],t1Packed);

            dAux0Advected = _mm_set_ps(d0[IX(i0[3],j0[3])],d0[IX(i0[2],j0[2])],d0[IX(i0[1],j0[1])],d0[IX(i0[0],j0[0])]);
            dAux0Advected = _mm_mul_ps(dAux0Advected,t0Packed);
            dAux1Advected = _mm_set_ps(d0[IX(i0[3],j1[3])],d0[IX(i0[2],j1[2])],d0[IX(i0[1],j1[1])],d0[IX(i0[0],j1[0])]);
            dAux1Advected = _mm_mul_ps(dAux1Advected,t1Packed);
            aux0Gather    = _mm_set_ps(d0[IX(i1[3],j0[3])],d0[IX(i1[2],j0[2])],d0[IX(i1[1],j0[1])],d0[IX(i1[0],j0[0])]);
            aux0Gather    = _mm_mul_ps(aux0Gather,t0Packed);
            aux1Gather    = _mm_set_ps(d0[IX(i1[3],j1[3])],d0[IX(i1[2],j1[2])],d0[IX(i1[1],j1[1])],d0[IX(i1[0],j1[0])]);
            aux1Gather    = _mm_mul_ps(aux1Gather,t1Packed);

            dAux0Advected = _mm_add_ps(dAux0Advected,dAux1Advected);
            dAux0Advected = _mm_mul_ps(dAux0Advected,s0Packed);
            aux0Gather    = _mm_add_ps(aux0Gather,aux1Gather);
            aux0Gather    = _mm_mul_ps(aux0Gather,s1Packed);
            dAdvected     = _mm_add_ps(dAux0Advected,aux0Gather);
                            _mm_store_ps(&d[IX(i, j)],dAdvected);
        }
    }
    set_bnd(n, b, d);

}

static void project(unsigned int n, float *u, float *v, float *p, float *div)
{
    __m128 uUpper;
    __m128 uLower;
    __m128 vPrev; 
    __m128 vPost; 
    __m128 vCenter;
    __m128 divProjected;
    __m128 vTempCenter;
    __m128 cte; 
    
    cte = _mm_set1_ps(- 0.5f/n); //cargo la cte
    //hacer la cruz, por eso son 5
    
    for (unsigned int i = 1; i <= n; i++) {
            uUpper       = _mm_load_ps(&u[IX(i-1, 0)]);
            uLower       = _mm_load_ps(&u[IX(i+1, 0)]);
            vPost        = _mm_load_ps(&v[IX(i, 4)]);
            vCenter      = _mm_load_ps(&v[IX(i, 0)]);
            
            divProjected = _mm_sub_ps(uLower,uUpper); 
            vCenter      = _mm_shuffle_ps(vCenter,vCenter,0x93);//los corro a la derecha para sumar el de la izquierda
            divProjected = _mm_sub_ps(divProjected,vCenter);//sumo los de la izquierda (borde tambien, despues hago set_bounds)
            vCenter      = _mm_shuffle_ps(vCenter,vCenter,0x39);//vuelvo a la posicion anterior, hacia la izquierda
            vCenter      = _mm_sub_ss(vCenter,vCenter);//pongo en cero el borde para que no me sume nada al ultimo elemento
            vCenter      = _mm_shuffle_ps(vCenter,vCenter,0x39);//los corro a la izquierda de nuevo
            divProjected = _mm_add_ps(divProjected,vCenter);//sumo los de la derecha (al ultimo le sumo cero)
            divProjected = _mm_shuffle_ps(divProjected,divProjected,0x93);//corro a la derecha para dejar el ultimo primero
            divProjected = _mm_add_ss(divProjected,vPost);//le sumo el que seria el de la derecha del ultimo
            divProjected = _mm_shuffle_ps(divProjected,divProjected,0x39);//corro a la izquierda para acomodar como estaban originalmente
            divProjected = _mm_mul_ps(divProjected,cte);//multiplico por cte a
                           _mm_store_ps(&div[IX(i, 0)],divProjected);//guardo    
            p[IX(i, 0)] = 0;
            p[IX(i, 1)] = 0;
            p[IX(i, 2)] = 0;
            p[IX(i, 3)] = 0;

        for (unsigned int j = 4; j <n-2; j+=4) 
        {
                //preparo para el siguiente 
                uUpper       = _mm_load_ps(&u[IX(i-1, j)]);
                uLower       = _mm_load_ps(&u[IX(i+1, j)]);
                vPrev        = _mm_load_ps(&v[IX(i, j-4)]);//lo cargo al reves para ya tenerlo listo para la suma
                vCenter      = _mm_load_ps(&v[IX(i, j)]);
                vTempCenter  = _mm_load_ps(&v[IX(i, j)]);
                vPost        = _mm_load_ps(&v[IX(i, j+4)]);

                //nomenclatura: ...x] [a,b,c,d] [y...
                divProjected = _mm_sub_ps(uLower,uUpper); 
                vCenter      = _mm_sub_ss(vCenter,vCenter); //pongo a cero el primer elemento de xCenter
                vCenter      = _mm_add_ss(vCenter,vPost); //guardo y en xCenter, queda [y,b,c,d]
                vCenter      = _mm_shuffle_ps(vCenter,vCenter,0x39);//los corro a la izquierda, quedan [b,c,d,y]
                vTempCenter  = _mm_shuffle_ps(vTempCenter,vTempCenter,0x93);//los corro a la izquierda, quedan [d,a,b,c]
                vTempCenter  = _mm_sub_ss(vTempCenter,vTempCenter); //pongo a cero el primer elemento de xTempCenter
                vTempCenter  = _mm_add_ss(vTempCenter,_mm_shuffle_ps(vPrev,vPrev,0x1B)); //guardo x en xTempCenter, queda [x,a,b,c]
                vCenter      = _mm_sub_ps(vCenter,vTempCenter); //sumo y me queda [b-x,c-a,d-b,y-c] (reste post - prev)
                divProjected = _mm_add_ps(divProjected,vCenter); //sumo al total 
                divProjected = _mm_mul_ps(divProjected,cte);//multiplico por cte
                               _mm_store_ps(&div[IX(i, j)],divProjected);//guardo
            /*div[IX(i, j)] = -0.5f/n * (u[IX(i + 1, j)] - u[IX(i - 1, j)] +
                                     v[IX(i, j + 1)] - v[IX(i, j - 1)]);*/
            p[IX(i, j  )] = 0;
            p[IX(i, j+1)] = 0;
            p[IX(i, j+2)] = 0;
            p[IX(i, j+3)] = 0;
        }
            //preparo para el siguiente 
            uUpper       = _mm_load_ps(&u[IX(i-1, n-2)]);
            uLower       = _mm_load_ps(&u[IX(i+1, n-2)]);
            vPrev        = _mm_load_ps(&v[IX(i, n-6)]);//lo cargo al reves para ya tenerlo listo para la suma
            vCenter      = _mm_load_ps(&v[IX(i, n-2)]); 

            divProjected = _mm_sub_ps(uLower,uUpper); 
            vCenter      = _mm_shuffle_ps(vCenter,vCenter,0x39);//los corro a la izquierda para sumar el de la derecha
            divProjected = _mm_add_ps(divProjected,vCenter);//sumo los de la derecha (borde tambien, no importa, despues hago set_bounds)
            vCenter      = _mm_shuffle_ps(vCenter,vCenter,0x4E);//muevo 2 a la derecha
            vCenter      = _mm_sub_ss(vCenter,vCenter);//pongo en cero el borde para que no me sume nada al ultimo elemento
            divProjected = _mm_sub_ps(divProjected,vCenter);//resto los de la izquierda (al primero le resto cero)
            divProjected = _mm_sub_ss(divProjected,_mm_shuffle_ps(vPrev,vPrev,0x1B));//resto el que seria el de la izquierda del primero
            divProjected = _mm_mul_ps(divProjected,cte);//multiplico por cte
                           _mm_store_ps(&div[IX(i, n-2)],divProjected);//guardo
            p[IX(i, n-2)] = 0;
            p[IX(i, n-1)] = 0;
            p[IX(i, n  )] = 0;
            p[IX(i, n+1)] = 0;
                                 
    }
    set_bnd(n, 0, div);
    set_bnd(n, 0, p);

    lin_solve(n, 0, p, div, 1, 4);

    __m128 pUpper;
    __m128 pLower;
    __m128 pPrev; 
    __m128 pPost; 
    __m128 pCenter;
    __m128 pTempCenter;
    __m128 uProjected;
    __m128 vProjected;

    cte = _mm_set1_ps(0.5f * n); //cargo la cte

    for (unsigned int i = 1; i <= n; i++) 
    {
            pUpper       = _mm_load_ps(&p[IX(i-1, 0)]);
            pLower       = _mm_load_ps(&p[IX(i+1, 0)]);
            pPost        = _mm_load_ps(&p[IX(i, 4)]);
            pCenter      = _mm_load_ps(&p[IX(i, 0)]);
            vProjected   = _mm_setzero_ps();  
            
            uProjected   = _mm_sub_ps(pLower,pUpper);//hago el de abajo menos el de arriba
            uProjected   = _mm_mul_ps(uProjected,cte);//multiplico por cte
            uProjected   = _mm_sub_ps(_mm_load_ps(&u[IX(i, 0)]),uProjected);
                           _mm_store_ps(&u[IX(i, 0)],uProjected);//guardo    
                           
            pCenter      = _mm_shuffle_ps(pCenter,pCenter,0x93);//los corro a la derecha para restar el de la izquierda
            vProjected   = _mm_sub_ps(vProjected,pCenter);//resto los de la izquierda (borde tambien, despues hago set_bounds)
            pCenter      = _mm_shuffle_ps(pCenter,pCenter,0x39);//vuelvo a la posicion anterior, hacia la izquierda
            pCenter      = _mm_sub_ss(pCenter,pCenter);//pongo en cero el borde para que no me sume nada al ultimo elemento
            pCenter      = _mm_shuffle_ps(pCenter,pCenter,0x39);//los corro a la izquierda de nuevo
            vProjected   = _mm_add_ps(vProjected,pCenter);//sumo los de la derecha (al ultimo le sumo cero)
            vProjected   = _mm_shuffle_ps(vProjected,vProjected,0x93);//corro a la derecha para dejar el ultimo primero
            vProjected   = _mm_add_ss(vProjected,pPost);//le sumo el que seria el de la derecha del ultimo
            vProjected   = _mm_shuffle_ps(vProjected,vProjected,0x39);//corro a la izquierda para acomodar como estaban originalmente
            vProjected   = _mm_mul_ps(vProjected,cte);//multiplico por cte
            vProjected   = _mm_sub_ps(_mm_load_ps(&v[IX(i, 0)]),vProjected);
                           _mm_store_ps(&v[IX(i, 0)],vProjected);//guardo

        for (unsigned int j = 4; j <n-2; j+=4) 
        {
            pUpper       = _mm_load_ps(&p[IX(i-1, j)]);
            pLower       = _mm_load_ps(&p[IX(i+1, j)]);
            pPrev        = _mm_load_ps(&p[IX(i, j-4)]);
            pCenter      = _mm_load_ps(&p[IX(i, j)]);
            pTempCenter  = _mm_load_ps(&p[IX(i, j)]);
            pPost        = _mm_load_ps(&p[IX(i, j+4)]);
            
            uProjected   = _mm_sub_ps(pLower,pUpper);
            uProjected   = _mm_mul_ps(uProjected,cte);//multiplico por cte a
            uProjected   = _mm_sub_ps(_mm_load_ps(&u[IX(i, j)]),uProjected);
                           _mm_store_ps(&u[IX(i, j)],uProjected);//guardo    
                           
            pCenter      = _mm_sub_ss(pCenter,pCenter); //pongo a cero el primer elemento de pCenter
            pCenter      = _mm_add_ss(pCenter,pPost); //guardo y en pCenter, queda [y,b,c,d]
            pCenter      = _mm_shuffle_ps(pCenter,pCenter,0x39);//los corro a la izquierda, quedan [b,c,d,y]
            pTempCenter  = _mm_shuffle_ps(pTempCenter,pTempCenter,0x93);//los corro a la derecha, quedan [d,a,b,c]
            pTempCenter  = _mm_sub_ss(pTempCenter,pTempCenter); //pongo a cero el primer elemento de pTempCenter
            pTempCenter  = _mm_add_ss(pTempCenter,_mm_shuffle_ps(pPrev,pPrev,0x1B)); //guardo x en pTempCenter, queda [x,a,b,c]
            vProjected   = _mm_sub_ps(pCenter,pTempCenter); //sumo y me queda [b-x,c-a,d-b,y-c] (reste post - prev)
            vProjected   = _mm_mul_ps(vProjected,cte);//multiplico por cte
            vProjected   = _mm_sub_ps(_mm_load_ps(&v[IX(i, j)]),vProjected);            
                           _mm_store_ps(&v[IX(i, j)],vProjected);//guardo
        }
        //preparo para el siguiente 
        pUpper       = _mm_load_ps(&p[IX(i-1, n-2)]);
        pLower       = _mm_load_ps(&p[IX(i+1, n-2)]);
        pPrev        = _mm_load_ps(&p[IX(i, n-6)]);
        pCenter      = _mm_load_ps(&p[IX(i, n-2)]);
        vProjected   = _mm_setzero_ps();
        
        uProjected   = _mm_sub_ps(pLower,pUpper);
        uProjected   = _mm_mul_ps(uProjected,cte);//multiplico por cte a
        uProjected   = _mm_sub_ps(_mm_load_ps(&u[IX(i, n-2)]),uProjected);
                       _mm_store_ps(&u[IX(i, n-2)],uProjected);//guardo   

        pCenter      = _mm_shuffle_ps(pCenter,pCenter,0x39);//los corro a la izquierda para sumar el de la derecha
        vProjected   = _mm_add_ps(vProjected,pCenter);//sumo los de la derecha (borde tambien, no importa, despues hago set_bounds)
        pCenter      = _mm_shuffle_ps(pCenter,pCenter,0x4E);//muevo 2 a la derecha
        pCenter      = _mm_sub_ss(pCenter,pCenter);//pongo en cero el borde para que no me sume nada al ultimo elemento
        vProjected   = _mm_sub_ps(vProjected,pCenter);//resto los de la izquierda (al primero le resto cero)
        vProjected   = _mm_sub_ss(vProjected,_mm_shuffle_ps(pPrev,pPrev,0x1B));//resto el que seria el de la izquierda del primero
        vProjected   = _mm_mul_ps(vProjected,cte);//multiplico por cte
        vProjected   = _mm_sub_ps(_mm_load_ps(&v[IX(i, n-2)]),vProjected);
                       _mm_store_ps(&v[IX(i, n-2)],vProjected);//guardo
    }
    set_bnd(n, 1, u);
    set_bnd(n, 2, v);
}

void dens_step(unsigned int n, float *x, float *x0, float *u, float *v, float diff, float dt)
{
    add_source(n, x, x0, dt);
    SWAP(x0, x);
    diffuse(n, 0, x, x0, diff, dt);
    SWAP(x0, x);
    advect(n, 0, x, x0, u, v, dt);
}

void vel_step(unsigned int n, float *u, float *v, float *u0, float *v0, float visc, float dt)
{

    add_source(n, u, u0, dt);
    add_source(n, v, v0, dt);
    SWAP(u0, u);
    diffuse(n, 1, u, u0, visc, dt);
    SWAP(v0, v);
    diffuse(n, 2, v, v0, visc, dt);
    project(n, u, v, u0, v0);
    SWAP(u0, u);
    SWAP(v0, v);
    advect(n, 1, u, u0, u0, v0, dt);
    advect(n, 2, v, v0, u0, v0, dt);
    project(n, u, v, u0, v0);
}
